import cv2
import numpy as np
import os


import logging

class Calibration:
    def __init__(self, camera_intrinsics=None, log_file="./logs/calibration_param_log.txt"):
        # 设置日志
        os.makedirs(os.path.dirname(log_file), exist_ok=True)
        self.logger = logging.getLogger("CalibrationLogger")
        self.logger.setLevel(logging.INFO)
        formatter = logging.Formatter('%(asctime)s %(message)s', datefmt='%Y-%m-%d %H:%M:%S')
        file_handler = logging.FileHandler(log_file, encoding="utf-8")
        file_handler.setFormatter(formatter)
        # 避免重复添加handler
        if not self.logger.handlers:
            self.logger.addHandler(file_handler)
        self.log_file = log_file

        # self.K = np.array([[744.645264, 0.000000, 619.264444],
        #                    [0.000000, 742.728702, 390.533505],
        #                    [0.000000, 0.000000, 1.000000]])
        self.K = np.array([[608.255915,0.000000 ,638.519334],
                [0.000000 ,609.206730, 365.428700],
                [0.000000 ,0.000000, 1.000000]])
        self.rgb_resolution = (1280, 720)
        self.depth_resolution = (1280, 720)
        
        # 计算缩放比例
        self.scale_x = self.rgb_resolution[0] / self.depth_resolution[0]
        self.scale_y = self.rgb_resolution[1] / self.depth_resolution[1]

        self.depth_scale = 1000.0

        # 初始化时记录参数到log
        self._log_parameters("初始化")

        self.depth_img=None

    def set_camera_intrinsics(self, camera_intrinsics):
        if not isinstance(camera_intrinsics, np.ndarray):
            print("输入参数类型错误，camera_intrinsics 需要为 numpy.ndarray 类型。")
            return
        if camera_intrinsics.shape != (3, 3):
            print("输入参数形状错误，camera_intrinsics 需要为 3x3 的矩阵。")
            return
        self.K = camera_intrinsics
        # 记录修改后的内参到log
        self._log_parameters("设置相机内参")

    def set_resolution(self, img, depth_img,depth_scale=1000.0):
        self.rgb_resolution = (img.shape[1], img.shape[0])
        self.depth_resolution = (depth_img.shape[1], depth_img.shape[0])

        self.scale_x = self.rgb_resolution[0] / self.depth_resolution[0]
        self.scale_y = self.rgb_resolution[1] / self.depth_resolution[1]
        self.depth_scale=depth_scale
        # 记录修改后的分辨率和缩放比例到log
        self._log_parameters("设置分辨率")

    def _log_parameters(self, action="参数变更"):
        """将当前参数写入log文件"""
        msg = (
            f"\n[{action}]\n"
            f"K:\n{self.K}\n"
            f"rgb_resolution: {self.rgb_resolution}\n"
            f"depth_resolution: {self.depth_resolution}\n"
            f"scale_x: {self.scale_x}\n"
            f"scale_y: {self.scale_y}\n"
            f"depth_scale: {self.depth_scale}\n"
            + "-" * 40
        )
        self.logger.info(msg)


    def set_depth_img(self,depth_img):
        self.depth_img=cv2.resize(depth_img, self.rgb_resolution, 
                                 interpolation=cv2.INTER_LINEAR)
        return self.depth_img
    def get_depth(self, rgb_x, rgb_y, edge_name=None):
        if self.depth_img is None:
            print(f"错误：缺少深度图。请使用set_depth_img(depth_img)设置深度图")
            return 0
        
        h, w = self.depth_img.shape

        # 检查坐标是否超出范围
        if not (0 <= rgb_x < w and 0 <= rgb_y < h):
            print(f"错误：输入坐标 (x={rgb_x}, y={rgb_y}) 超出图像范围 (宽度: {w}, 高度: {h})")
            return 0

        # 1. 直接获取深度值（如果有效）
        direct_depth = self.depth_img[rgb_y, rgb_x]
        if direct_depth > 0:
            return direct_depth
        elif edge_name is not None:
            #TODO: 记录log 查找edge_name的深度值
            depth=self._get_depth_by_edge_name(rgb_x,rgb_y,edge_name)
            msg = (
                f"{edge_name} 获取深度 {depth}\n"
            )
            self.logger.info(msg)
            return depth
        else:
            return 0
        
    def _get_depth_by_edge_name(self,rgb_x,rgb_y,edge_name):
        h, w = self.depth_img.shape
        
        x, y = rgb_x, rgb_y
        found = False

        if edge_name == "left_edge":
            # 向右查找
            nx = x
            while nx < w:
                depth = self.depth_img[y, nx]
                if 0 < depth < 10000:
                    found = True
                    break
                nx += 1
            if found:
                nx2 = min(nx + 2, w - 1)
                depth2 = self.depth_img[y, nx2]
                if 0 < depth2 < 10000:
                    return depth2
                else:
                    return self.depth_img[y, nx]
            else:
                return 0

        elif edge_name == "right_edge":
            # 向左查找
            nx = x
            while nx >= 0:
                depth = self.depth_img[y, nx]
                if 0 < depth < 10000:
                    found = True
                    break
                nx -= 1
            if found:
                nx2 = max(nx - 2, 0)
                depth2 = self.depth_img[y, nx2]
                if 0 < depth2 < 10000:
                    return depth2
                else:
                    return self.depth_img[y, nx]
            else:
                return 0

        elif edge_name == "bottom_edge":
            # 向上查找
            ny = y
            while ny >= 0:
                depth = self.depth_img[ny, x]
                if 0 < depth < 10000:
                    found = True
                    break
                ny -= 1
            if found:
                ny2 = max(ny - 2, 0)
                depth2 = self.depth_img[ny2, x]
                if 0 < depth2 < 10000:
                    return depth2
                else:
                    return self.depth_img[ny, x]
            else:
                return 0

        elif edge_name == "top_edge":
            # 向下查找
            ny = y
            while ny < h:
                depth = self.depth_img[ny, x]
                if 0 < depth < 10000:
                    found = True
                    break
                ny += 1
            if found:
                ny2 = min(ny + 2, h - 1)
                depth2 = self.depth_img[ny2, x]
                if 0 < depth2 < 10000:
                    return depth2
                else:
                    return self.depth_img[ny, x]
            else:
                return 0

        # 如果没有指定edge_name或找不到有效值
        return 0

    def pixel_to_3d(self, u, v, depth):
        """将像素坐标和深度转换为3D坐标"""
        if depth <= 0:
            return None
        
        depth_in_meters = depth / self.depth_scale
        
        fx, fy = self.K[0, 0], self.K[1, 1]
        cx, cy = self.K[0, 2], self.K[1, 2]
        
        x = (u - cx) * depth_in_meters / fx
        y = (v - cy) * depth_in_meters / fy
        z = depth_in_meters
        
        return np.array([x, y, z])
